/*
 * @Description: 订阅激光点云信息，并解析数据
 * @Author: Zhijian Qiao
 * @Date: 2020-02-05 02:27:30
 */

#include "subscriber/four_images_subscriber.hpp"
#include<ros/ros.h>
#include "glog/logging.h"
#include <message_filters/time_synchronizer.h>
#include <boost/bind.hpp>
#include <boost/filesystem.hpp>
#include <cv_bridge/cv_bridge.h>

namespace avp_slam {
    FourImagesSubscriber::FourImagesSubscriber(ros::NodeHandle &nh, size_t buff_size)
            : nh_(nh) {

        cmr0_ = std::make_shared<filter_image>(nh, "/robot_1/camera0/depth/image_rect_raw", buff_size);
        cmr1_ = std::make_shared<filter_image>(nh, "/robot_1/camera0/color/image_raw", buff_size);
        cmr2_ = std::make_shared<filter_image>(nh, "/robot_1/camera1/depth/image_rect_raw", buff_size);
        cmr3_ = std::make_shared<filter_image>(nh, "/robot_1/camera1/color/image_raw", buff_size);
        cmr4_ = std::make_shared<filter_image>(nh, "/robot_1/camera2/depth/image_rect_raw", buff_size);
        cmr5_ = std::make_shared<filter_image>(nh, "/robot_1/camera2/color/image_raw", buff_size);
        cmr6_ = std::make_shared<filter_image>(nh, "/robot_1/camera3/depth/image_rect_raw", buff_size);
        cmr7_ = std::make_shared<filter_image>(nh, "/robot_1/camera3/color/image_raw", buff_size);

        appro_synch_ = std::make_shared<message_filters::Synchronizer<ApproSynch>>(ApproSynch(10), *cmr0_, *cmr1_,
                                                                                   *cmr2_, *cmr3_, *cmr4_, *cmr5_,
                                                                                   *cmr6_, *cmr7_);
        appro_synch_->registerCallback(
                boost::bind(&FourImagesSubscriber::msg_callback, this, _1, _2, _3, _4, _5, _6, _7, _8));
    }

    void FourImagesSubscriber::msg_callback(const sensor_msgs::ImageConstPtr &depthFront,
                                            const sensor_msgs::ImageConstPtr &colorFront,
                                            const sensor_msgs::ImageConstPtr &depthLeft,
                                            const sensor_msgs::ImageConstPtr &colorLeft,
                                            const sensor_msgs::ImageConstPtr &depthBack,
                                            const sensor_msgs::ImageConstPtr &colorBack,
                                            const sensor_msgs::ImageConstPtr &depthRight,
                                            const sensor_msgs::ImageConstPtr &colorRight) {
        buff_mutex_.lock();
//        假设所有图像时间大致相等
        double time = depthFront->header.stamp.toSec();
//        LOG(INFO) << std::fixed << "image time: "<<time;
        FourImagesData four_data(time, cv_bridge::toCvShare(depthFront)->image,
                                 cv_bridge::toCvShare(colorFront)->image, cv_bridge::toCvShare(depthLeft)->image,
                                 cv_bridge::toCvShare(colorLeft)->image, cv_bridge::toCvShare(depthBack)->image,
                                 cv_bridge::toCvShare(colorBack)->image, cv_bridge::toCvShare(depthRight)->image,
                                 cv_bridge::toCvShare(colorRight)->image);
        new_images_data_.push_back(four_data);
        buff_mutex_.unlock();
    }

    void FourImagesSubscriber::ParseData(std::deque<FourImagesData> &images_data_buff) {
        buff_mutex_.lock();
        if (new_images_data_.size() > 0) {
            images_data_buff.insert(images_data_buff.end(), new_images_data_.begin(), new_images_data_.end());
            new_images_data_.clear();
        }
        buff_mutex_.unlock();
    }

} // namespace data_input